#ifndef SENSOR_HPP
#define SENSOR_HPP
#include <string>
#include <vector>
#include <iostream>
#include <iomanip> // 用于setw和setfill
#include "serial.hpp"
// 输出函数
void printHexVector(const std::vector<uint8_t>& msg) {
    // 设置16进制输出，填充字符为'0'，宽度为2
    std::cout << std::hex << std::setfill('0');
    for (uint8_t byte : msg) {
        // 确保每个字节用两位16进制数表示
        std::cout << std::setw(2) << static_cast<int>(byte) << " ";
    }
    std::cout << std::dec; // 恢复为十进制输出，避免影响后续输出
    std::cout << std::endl;
}

class Sensor_Drv {
    public:
        Sensor_Drv(std::string port, int baud)
        {
            serial_.open(port, baud); 
             // 设置串口回调，将收到的数据转发给本类的recvMsg
        serial_.set_receive_callback([this](const std::vector<uint8_t>& msg) {
            this->recvMsg(const_cast<std::vector<uint8_t>&>(msg));
        });
        startTimer(); // 启动定时器
        }
         ~Sensor_Drv() 
         {
            stopTimer();
            serial_.close();
         }
        void stopTimer() {
        running_ = false;
        if (timer_thread_.joinable()) timer_thread_.join();
       }
    //private:
         SerialPort serial_;
         void startTimer(void);
         std::thread timer_thread_;
         bool running_=false; // Flag to indicate if the timer is running
         std::vector<u_int8_t> sendbuf_vect; // Buffer for sending data
         std::vector<u_int8_t> recvbuf_vect; // Buffer for receiving data
         int delay_ms = 10; // Default delay in milliseconds
         int task_switch = 0; // Task switch variable
         std::vector<u_int8_t> rec_allmsg;
         float distance0,distance1;
         float distance2,distance3,distance4,distance5;
         int sensor0_id,sensor1_id;   //0x181;
         int sensor2_id,sensor3_id,sensor4_id,sensor5_id; ///抓钩四周传感器
         int motor_id=0x171;
         int zhuagou_id,zhuagou_cmd;
         int motor_cur_speed,motor_set_speed;
         void get_recv_msg(std::vector<u_int8_t> &msg);
         void recvMsg(std::vector<u_int8_t> &msg);
         int get_send(int tk_switch);
         int rec_ok(std::vector<u_int8_t> &msg);

};
void Sensor_Drv::startTimer(void)
{
     if (running_) return;
        running_ = true;
        //int ms = 10; // Default interval
        
        timer_thread_ = std::thread([this]() {
            while (running_) {
                //std::string hello = "hello";
                sendbuf_vect.clear(); // Clear the send buffer before sending
                delay_ms = get_send(task_switch); // Example delay, can be set dynamically
                //int delay_ms = 10; // Default delay in milliseconds
                if(sendbuf_vect.size() > 0) { // Check if there is data to send
                    //std::cout << "Sending data: " << sendbuf_vect.size() << " bytes" << std::endl;
                    serial_.send(sendbuf_vect);
                } 
                
                std::chrono::milliseconds interval;
               // delay_ms = 20; // Example delay, can be set dynamically
                interval = std::chrono::milliseconds(delay_ms);
                std::this_thread::sleep_for(interval);
                task_switch++;
                if(task_switch>8){
                    task_switch=0;
                  //  config_.yaokonglun();
                }
              //  std::cout << "Timer tick, delay: " << delay_ms << " ms" << std::endl;
            }
        });
}
void Sensor_Drv::get_recv_msg(std::vector<u_int8_t> &msg)
{
    int16_t id,ag;
    float dis;
    if(rec_ok(msg) != 0) {
         id=msg[1]*256+msg[2];
         ag=msg[3]*256+msg[4];
         if(id==(sensor0_id+0x100)){
          dis=ag; distance0=dis/1000;
          //std::cout<<"id= "<<id<<" dis0="<<distance0<<std::endl;
         }
         if(id==(sensor1_id+0x100)){
          dis=ag; distance1=dis/1000;
          //std::cout<<"id= "<<id<<" dis1="<<distance1<<std::endl;
         }
         if(id==(sensor2_id+0x100)){
          dis=ag; distance2=dis/1000;
          //std::cout<<"id= "<<id<<" dis2="<<distance2<<std::endl;
         }
         if(id==(sensor3_id+0x100)){
          dis=ag; distance3=dis/1000;
          //std::cout<<"id= "<<id<<" dis0="<<distance0<<std::endl;
         }
         if(id==(sensor4_id+0x100)){
          dis=ag; distance4=dis/1000;
          //std::cout<<"id= "<<id<<" dis0="<<distance0<<std::endl;
         }
         if(id==(sensor5_id+0x100)){
          dis=ag; distance5=dis/1000;
          //std::cout<<"id= "<<id<<" dis0="<<distance0<<std::endl;
         }
         if(id==(motor_id+0x100))
         {
           id=msg[5]*256+msg[6];
           motor_cur_speed = id;
          // std::cout<<"motor id="<<id<<" ag="<<ag<<std::endl;
         }
         if(id==(zhuagou_id+0x100))
         {
           id=msg[5]*256+msg[6];
           //motor_cur_speed = id;
           //std::cout<<"抓钩 id="<<zhuagou_id<<" ag="<<ag<<std::endl;
         }
      //   std::cout<<"receive "<<std::endl;
    }
}
void Sensor_Drv::recvMsg(std::vector<u_int8_t> &msg)
{
   // printHexVector(msg);
    rec_allmsg.insert(rec_allmsg.end(), msg.begin(), msg.end()); // Append received message to the buffer
    while (rec_allmsg.size() >= 12) { // Process messages in chunks of 12 bytes
        {
            if (rec_ok(rec_allmsg) != 0) {
                get_recv_msg(rec_allmsg);
                rec_allmsg.erase(rec_allmsg.begin(), rec_allmsg.begin() + 12);
            }
            else rec_allmsg.erase(rec_allmsg.begin()); // Remove processed message
        }
     }
     // std::cout<<"receive "<<std::endl;
}
int Sensor_Drv::rec_ok(std::vector<u_int8_t> &msg)
{
     int flag=0;
     u_int8_t ch=0;
    if(msg[0]==0xaa)  //can 口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<11;i++)
        ch+=msg[i];
        if(ch==msg[11])flag=1;  
    }
    if(msg[0]==0xbb)    //can 口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<11;i++)
        ch+=msg[i];
        if(ch==msg[11])flag=2;  
    }  
    if(msg[0]==0xcc)  //串口口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<10;i++)
        ch+=msg[i];
       if(ch==msg[10])
        flag=3;  
    }
  
        return flag;
}
int Sensor_Drv::get_send(int tk_switch)
{
    int id,delay;
    u_int8_t ch=0;
     delay=4;
    if(tk_switch==1)  //电机控制
    {
            sendbuf_vect.push_back(0xaa);
            id=motor_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x03); //力矩模式
            sendbuf_vect.push_back(0x01);  //电机使能

            sendbuf_vect.push_back((motor_set_speed>>8)&0xff);
            sendbuf_vect.push_back(motor_set_speed&0xff);

            sendbuf_vect.push_back(0x27); //力矩开到最大
            sendbuf_vect.push_back(0x10);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
    }
   if(tk_switch==2)  //测距传感器0
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor0_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
  if(tk_switch==3)  //测距传感器1
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor1_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
   if(tk_switch==4)  //抓钩电机
   {
            sendbuf_vect.push_back(0xaa);
            id=zhuagou_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            id=zhuagou_cmd;
            sendbuf_vect.push_back(id&0x3);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
   if(tk_switch==5)  //测距传感器2  抓钩
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor2_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
   if(tk_switch==6)  //测距传感器3  抓钩
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor3_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
   if(tk_switch==7)  //测距传感器3  抓钩
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor4_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           // std::cout<<"send byte "<<std::endl;
   }
   if(tk_switch==8)  //测距传感器4  抓钩
   {
            sendbuf_vect.push_back(0xaa);
            id=sensor5_id;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
         //   std::cout<<"send byte 8"<<std::endl;
   }
   return delay;
}
#endif
